#include "jps_planner.h"

namespace roborts_global_planner{

using roborts_common::ErrorCode;
using roborts_common::ErrorInfo;

JpsPlanner::JpsPlanner(CostmapPtr costmap_ptr)
:costmap_(NULL)
,costmap_ptr_(costmap_ptr)
,GlobalPlannerBase::GlobalPlannerBase(costmap_ptr)
,jps_planner_()
,initialized_(false)
{
    initialize(costmap_ptr);
}

void JpsPlanner::initialize(CostmapPtr costmap_ptr)
{
    if(!initialized_)
    {
        costmap_ = costmap_ptr->GetCostMap();
        map_util_ = std::make_shared<JPS::OccMapUtil>();

        ///////////////////////////////////////////////

        const Vec2f map_origin(costmap_->GetOriginX(), costmap_->GetOriginY());
        const Vec2i map_dimension(costmap_->GetSizeXCell(), costmap_->GetSizeYCell());
        const int32_t dimension(costmap_->GetSizeXCell() * costmap_->GetSizeYCell());

        unsigned char* map_array = costmap_->GetCharMap();
        JPS::Tmap map_data;
        map_data.resize(dimension);
        int occ = 0;
        for (unsigned int i = 0; i < dimension; i++)
        {
            map_data[i] = map_array[i] > 254 ? 1 : 0;

        }

        const decimal_t map_resolution = costmap_->GetResolution();
        
        map_util_->setMap(
            map_origin,     // origin position
            map_dimension,  // number of cells in each dimension
            map_data,       // map resolution
            map_resolution);

        if (debug_) map_util_->info();

        ///////////////////////////////////////////////
        // Set up JPS planner
        jps_planner_ = std::make_shared<JPSPlanner2D>(debug_);
        jps_planner_->setMapUtil(map_util_);  // Set collision checking function
        jps_planner_->updateMap();       

        global_frame_ = costmap_ptr->GetGlobalFrameID();

        initialized_ = true;
    }
    else
        ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
}

JpsPlanner::~JpsPlanner(){
    
}

ErrorInfo JpsPlanner::Plan(const geometry_msgs::PoseStamped &start,
                                 const geometry_msgs::PoseStamped &goal,
                                 std::vector<geometry_msgs::PoseStamped> &path){

  boost::mutex::scoped_lock lock(mutex_);
  if (!initialized_)
  {
    ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
    return ErrorInfo(ErrorCode::GP_INITILIZATION_ERROR,"Planner hasn't been initialized");
  }
        const Vec2f map_origin(costmap_->GetOriginX(), costmap_->GetOriginY());
        const Vec2i map_dimension(costmap_->GetSizeXCell(), costmap_->GetSizeYCell());
        const int32_t dimension(costmap_->GetSizeXCell() * costmap_->GetSizeYCell());

        unsigned char* map_array = costmap_->GetCharMap();
        JPS::Tmap map_data;
        map_data.resize(dimension);
        int occ = 0;
        for (unsigned int i = 0; i < dimension; i++)
        {
            map_data[i] = map_array[i] > 99 ? 1 : 0;
            if (map_array[i]>99)
                occ++;
        }

        const decimal_t map_resolution = costmap_->GetResolution();
        
        map_util_->setMap(
            map_origin,     // origin position
            map_dimension,  // number of cells in each dimension
            map_data,       // map resolution
            map_resolution);
  // clear the plan, just in case
  path.clear();

  // until tf can handle transforming things that are way in the past...
  // we'll require the goal to be in our global frame
//   if (goal.header.frame_id != global_frame_)
//   {
//     std::cout<<goal <<"    "<< global_frame_<<std::endl;
//     ROS_ERROR("The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.",
//               global_frame_.c_str(), goal.header.frame_id.c_str());
//     return ErrorInfo(ErrorCode::GP_POSE_TRANSFORM_ERROR,"Goal is not in map frame");
//   }

//   if (start.header.frame_id != global_frame_)
//   {
//     ROS_ERROR("The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.",
//               global_frame_.c_str(), start.header.frame_id.c_str());
//     return ErrorInfo(ErrorCode::GP_POSE_TRANSFORM_ERROR,"Start is not in map frame");
//   }
    
  double wx = start.pose.position.x;
  double wy = start.pose.position.y;

  unsigned int mx, my;
  if (!costmap_->World2Map(wx, wy, mx, my))
  {
    ROS_WARN("Failed to transform start pose from map frame to costmap frame");
    return ErrorInfo(ErrorCode::GP_POSE_TRANSFORM_ERROR,
                     "Start pose can't be transformed to costmap frame.");
  }

  // Clear the starting cell within the costmap because we know it can't be an obstacle
  clearRobotCell(start, mx, my);

  /**
   * World: coordinates in the "map" frame
   * Map: discretized coordinates for the occupancy grid
   *
   */
  const Vec2f start_m(mx, my);
  const Vec2f start_w(wx, wy);

  ////////////////////////////////////////////

  int map_start[2];
  map_start[0] = mx;
  map_start[1] = my;

  wx = goal.pose.position.x;
  wy = goal.pose.position.y;

  if (!costmap_->World2Map(wx, wy, mx, my))
  {
    mx = 0;
    my = 0;
    ROS_WARN("Failed to transform goal pose from map frame to costmap frame");
    return ErrorInfo(ErrorCode::GP_POSE_TRANSFORM_ERROR,
                     "Goal pose can't be transformed to costmap frame.");
  }
  // ROS_WARN("cost:%d",costmap_->GetCost(mx, my));
  int map_goal[2];
  map_goal[0] = mx;
  map_goal[1] = my;

  const Vec2f goal_m(map_goal[0], map_goal[1]);
  const Vec2f goal_w(wx, wy);

  ROS_DEBUG("[JPS] Planning path from [%f, %f] to [%f, %f]",
      start_w[0], start_w[1], goal_w[0], goal_w[1]);

  jps_planner_->updateMap();

  bool result = jps_planner_->plan(start_w, goal_w, /*eps*/1, /*use_jps*/true);

  if (!result) {
        ROS_WARN("Global planner can't search the valid path!");
        return ErrorInfo(ErrorCode::GP_PATH_SEARCH_ERROR, "Valid global path not found.");
}

  // Get the planned raw path from JPS
  const vec_Vec2f path_jps = jps_planner_->getRawPath();

  path.clear();
  const std::size_t jps_path_size = path_jps.size();
  path.resize(jps_path_size);
  for (int i=0; i < jps_path_size-1; i++)
  {
    geometry_msgs::PoseStamped msg;
    msg.header.frame_id = "/map";
    msg.pose.position.x = path_jps[i](0);
    msg.pose.position.y = path_jps[i](1);
    msg.pose.orientation.w = 1;
    path[i] = msg;
  }
  path[path.size()-1] = goal;
  /***** get force neighbours*********/
//   const vec_Vec2f force_jps = jps_planner_->getForce();
//   nav_msgs::Path force_point;
//   force_point.header.frame_id = global_frame_;
//   force_point.header.stamp = ros::Time::now();
//   const std::size_t force_point_size = path_jps.size();
//   force_point.poses.resize(force_point_size);
//   for (int i=0; i < force_point_size; i++)
//   {
//     geometry_msgs::PoseStamped msg;
//     msg.pose.position.x = path_jps[i](0);
//     msg.pose.position.y = path_jps[i](1);
//     msg.pose.orientation.w = 1;
//     force_point.poses[i] = msg;
//   }
//   force_point_pub_.publish(force_point);

  // Publish the plan for visualization purposes
//   publishPlan(plan, 0.0, 1.0, 0.0, 0.0);

  return ErrorInfo(ErrorCode::OK);

}

void JpsPlanner::clearRobotCell(const geometry_msgs::PoseStamped& global_pose,
                                        unsigned int mx, unsigned int my)
{
  if (!initialized_)
  {
    ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
    return;
  }

  // Set the associated costs in the cost map to be free
  costmap_->SetCost(mx, my, roborts_costmap::FREE_SPACE);
}

}//namespace jps_planner

